  /*
   * NunchuckPrint Values in DEGREEs
   * 2007 Tod E. Kurt, http://todbot.com/blog/
   * The Wii Nunchuck reading code is taken from Windmeadow Labs
   * http://www.windmeadow.com/node/42
   * modified by crimony in http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1264805255/0#5 to read cheap ebay nunchucks
   * Valuable infos from http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
   * and http://rubenlaguna.com/wp/2008/11/03/arduino-tilt-sensor-using-mma7260q/
   * Last modification by ntgr (03-2-2010)
   */
   
  #include <Wire.h>
  uint8_t ctrlr_type[6]; 
  
  void setup()
  {
    Serial.begin(19200);
    nunchuck_setpowerpins(); // use analog pins 2&3 as fake gnd & pwr
    nunchuck_init(); // send the initilization handshake
    Serial.print ("Finished setup\n");
  }
  
  void loop()
  {
    nunchuck_get_data();
    nunchuck_print_data();
    delay(500);
  }
  
  
  //
  // Nunchuck functions
  //
  
  static uint8_t nunchuck_buf[6];   // array to store nunchuck data,
  
  // Uses port C (analog in) pins as power & ground for Nunchuck
  static void nunchuck_setpowerpins()
  {
  #define pwrpin PORTC3
  #define gndpin PORTC2
      DDRC |= _BV(pwrpin) | _BV(gndpin);
      PORTC &=~ _BV(gndpin);
      PORTC |=  _BV(pwrpin);
      delay(100);  // wait for things to stabilize        
  }
  
  // initialize the I2C system, join the I2C bus,
  // and tell the nunchuck we're talking to it
  void nunchuck_init()
  { 
    byte cnt;
  
  Wire.begin();
              
  // init controller
  delay(1);
  Wire.beginTransmission(0x52);      // device address
  Wire.write(0xF0);                    // 1st initialisation register
  Wire.write(0x55);                    // 1st initialisation value
  Wire.endTransmission();
  delay(1);
  Wire.beginTransmission(0x52);
  Wire.write(0xFB);                    // 2nd initialisation register
  Wire.write(0x00);                    // 2nd initialisation value
  Wire.endTransmission();
  delay(1);
              
  // read the extension type from the register block        
  Wire.beginTransmission(0x52);
  Wire.write(0xFA);                    // extension type register
  Wire.endTransmission();
  Wire.beginTransmission(0x52);
  Wire.requestFrom(0x52, 6);               // request data from controller
  for (cnt = 0; cnt < 6; cnt++) {
      if (Wire.available()) {
          ctrlr_type[cnt] = Wire.read(); // Should be 0x0000 A420 0101 for Classic Controller, 0x0000 A420 0000 for nunchuck
      }
  }
  Wire.endTransmission();
  delay(1);
              
  // send the crypto key (zeros), in 3 blocks of 6, 6 & 4.
  Wire.beginTransmission(0x52);
  Wire.write(0xF0);                    // crypto key command register
  Wire.write(0xAA);                    // sends crypto enable notice
  Wire.endTransmission();
  delay(1);
  Wire.beginTransmission(0x52);
  Wire.write(0x40);                    // crypto key data address
  for (cnt = 0; cnt < 6; cnt++) {
      Wire.write(0x00);                    // sends 1st key block (zeros)
  }
  Wire.endTransmission();
  Wire.beginTransmission(0x52);
  Wire.write(0x40);                    // sends memory address
  for (cnt = 6; cnt < 12; cnt++) {
      Wire.write(0x00);                    // sends 2nd key block (zeros)
  }
  Wire.endTransmission();
  Wire.beginTransmission(0x52);
  Wire.write(0x40);                    // sends memory address
  for (cnt = 12; cnt < 16; cnt++) {
      Wire.write(0x00);                    // sends 3rd key block (zeros)
  }
  Wire.endTransmission();
  delay(1);
  // end device init 
  
  }
  
  // Send a request for data to the nunchuck
  // was "send_zero()"
  void nunchuck_send_request()
  {
    Wire.beginTransmission(0x52);      // transmit to device 0x52
    Wire.write(0x00);            // sends one byte
    Wire.endTransmission();      // stop transmitting
  }
  
  // Receive data back from the nunchuck, 
  int nunchuck_get_data()
  {
      int cnt=0;
      Wire.requestFrom (0x52, 6);      // request data from nunchuck
      while (Wire.available ()) {
        // receive byte as an integer
        nunchuck_buf[cnt] = nunchuk_decode_byte(Wire.read());
        cnt++;
      }
      nunchuck_send_request();  // send request for next data payload
      // If we recieved the 6 bytes, then go print them
      if (cnt >= 5) {
       return 1;   // success
      }
      return 0; //failure
  }
  
  // Print the input data we have recieved
  // accel data is 10 bits long
  // so we read 8 bits, then we have to add
  // on the last 2 bits.  That is why I
  // multiply them by 2 * 2
  void nunchuck_print_data()
  { 
    static int i=0;
    int joy_x_axis = nunchuck_buf[0];
    int joy_y_axis = nunchuck_buf[1];
    int accel_x_axis = nunchuck_buf[2]; // Readings from My wii in X axis
    int accel_y_axis = nunchuck_buf[3]; // Readings from My wii in Y axis
    int accel_z_axis = nunchuck_buf[4]; // Readings from My wii in Z axis
  
    int z_button = 0;
    int c_button = 0;
    
    //*******************************************************************************************
    
    int minx = 78;       //Minimum Measured value in X axis as readed in my tests.
    int maxx = 182;      //Maximum Measured value in X axis
    int miny = 78;       //Minimum Measured value in Y axis
    int maxy = 182;      //Maximum Measured value in Y axis
    int minz = 78;       //Minimum Measured value in Z axis
    int maxz = 182;      //Maximum Measured value in Z axis
     
    int g0x;
    int g0y;
    int g0z;
    g0x = ((maxx - minx)/2)+minx;
    g0y = ((maxy - miny)/2)+miny;
    g0z = ((maxz - minz)/2)+minz;
    
    int fx = (accel_x_axis - g0x);
    int fy = (accel_y_axis - g0y);
    int fz = (accel_z_axis - g0z);
    
    float ax = fx*(3.3/(1024.0*((maxx-minx))/180));  //The 3.3V supply volt is divided by 1024 steps from the A/D converter. This value is divided by the sensitivity in X axis
    float ay = fy*(3.3/(1024.0*((maxy-miny))/180));  //The 3.3V supply volt is divided by 1024 steps from the A/D converter. This value is divided by the sensitivity in Y axis
    float az = fz*(3.3/(1024.0*((maxz-minz))/180));  //The 3.3V supply volt is divided by 1024 steps from the A/D converter. This value is divided by the sensitivity in Z axis
    
    float rho = 0;             //MyAngle in X Axis
    float phi = 0;             //MyAngle in Y Axis
    float theta = 0;           //MyAngle in Z Axis
    rho =   atan(ax/sqrt(pow(ay,2)+pow(az,2)))*(360/(2*3.1415));  //Calculate the X , Y ,Z angle.
    phi =   atan(ay/sqrt(pow(ax,2)+pow(az,2)))*(360/(2*3.1415));  // this used (360/(2*3.1415) to convert radians to degrees.
    theta = atan(sqrt(pow(ay,2)+pow(ax,2))/az)*(360/(2*3.1415));  //More in http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf (PAGE 4) 
    
    //*******************************************************************************************
  
    // byte nunchuck_buf[5] contains bits for z and c buttons
    // it also contains the least significant bits for the accelerometer data
    // so we have to check each bit of byte outbuf[5]
    if ((nunchuck_buf[5] >> 0) & 1) 
      z_button = 1;
    if ((nunchuck_buf[5] >> 1) & 1)
      c_button = 1;
  
    if ((nunchuck_buf[5] >> 2) & 1) 
      accel_x_axis += 2;
    if ((nunchuck_buf[5] >> 3) & 1)
      accel_x_axis += 1;
  
    if ((nunchuck_buf[5] >> 4) & 1)
      accel_y_axis += 2;
    if ((nunchuck_buf[5] >> 5) & 1)
      accel_y_axis += 1;
  
    if ((nunchuck_buf[5] >> 6) & 1)
      accel_z_axis += 2;
    if ((nunchuck_buf[5] >> 7) & 1)
      accel_z_axis += 1;
  
    Serial.print(i,DEC);
    Serial.print("\t");
    
    Serial.print("joy:");
    Serial.print(joy_x_axis,DEC);
    Serial.print(",");
    Serial.print(joy_y_axis, DEC);
    Serial.print("  \t");

    Serial.print("acc:");
    Serial.print(accel_x_axis, DEC);
    Serial.print(",");
    Serial.print(accel_y_axis, DEC);
    Serial.print(",");
    Serial.print(accel_z_axis, DEC);
    Serial.print("\t");
  
    Serial.print("but:");
    Serial.print(z_button, DEC);
    Serial.print(",");
    Serial.print(c_button, DEC);
  
    Serial.print("  \t");
    Serial.print(" ANGLE X --> ");
    Serial.print(rho, 2);
    Serial.print("  \t");
    Serial.print(" ANGLE Y --> ");
    Serial.print(phi, 2);
    Serial.print("  \t");
    Serial.print(" ANGLE Z --> ");
    Serial.print(theta, 2);
  
    Serial.print("\r\n");  // newline
    i++;
  }
  
  // Encode data to format that most wiimote drivers except
  // only needed if you use one of the regular wiimote drivers
  char nunchuk_decode_byte (char x)
  {
    x = (x ^ 0x17) + 0x17;
    return x;
  }
